from rclpy.node import Node
from example_ros2_interface.msg import RobotStatus
from example_ros2_interface.srv import MoveRobot
import rclpy
     
class Interface_contraller(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info('running itf_ctrlr node:{}'.format(name))
        self.client = self.create_client(MoveRobot,"move_robot") 
        self.robot_status_subscribe = self.create_subscription(RobotStatus,"robot_status",self.robot_status_callback,10)

    def robot_status_callback(self,msg):
        self.get_logger().info(f"receive status location：{msg.pose} status：{msg.status}")

    def move_result_callback(self, result_future):
        response = result_future.result()
        self.get_logger().info(f"收到返回结果：{response.pose}")

    def move_robot(self, distance):
        while rclpy.ok() and self.client.wait_for_service(1)==False:
            self.get_logger().info(f"wait server ...")
        request = MoveRobot.Request()
        request.distance = distance
        self.get_logger().info(f"request moving for robor{distance}")
        self.client.call_async(request).add_done_callback(self.move_result_callback)
def main(args=None):
    rclpy.init(args=args)
    node = Interface_contraller('itf_contraller')
    node.move_robot(5.0)
    rclpy.spin(node)
    rclpy.shutdown()